#include "WProgram.h"
void setup();
void loop();
void motorCommand(byte servo, unsigned int angle);
void motorConfigure(byte servo);
void servoOff(byte servo);
unsigned int data = 2000;

void setup() {
  Serial.begin(38400);
  delay(500);
  for (byte channel = 0; channel < 4; channel++) motorConfigure(channel);
}

void loop() {
  for (byte channel = 0; channel < 4; channel++) motorCommand(channel, data);
}

void motorCommand(byte servo, unsigned int angle){
   //servo is the servo number (typically 0-7)
   //angle is the absolute position from 2000 to 4695?

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x04,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   //Convert the angle data into two 7-bit bytes
   Serial.print(((angle>>7)&0x3f),BYTE); //data1
   Serial.print((angle&0x7f),BYTE); //data2
}

void motorConfigure(byte servo){
   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE); //start byte
   Serial.print(0x01,BYTE); //device id
   Serial.print(0x01,BYTE); //command number
   Serial.print(servo,BYTE); //servo number
   Serial.print(0x0,BYTE); //instant change
   
   motorCommand(servo, 2000);
}

void servoOff(byte servo){//turns off a servo
   //(servo will go limp until next position command)
   //servo is the servo number (typically 0-7)

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE);//start byte
   Serial.print(0x01,BYTE);//device id
   Serial.print(0x00,BYTE);//command number
   Serial.print(servo,BYTE);//servo number
   Serial.print(0x0f,BYTE);//data1 (turn servo off, keep full range) 
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

